#include "MyRobot.h"

//BEN NOTE:
//Optional software update here:
//http://www.usfirst.org/uploadedFiles/Robotics_Programs/FRC/Game_and_Season__Info/2011_Assets/Team_Updates/Team%20Update%2020.pdf

//BEN NOTE:
//To download cRIO images, first download them, then copy the entire zip 
//file into C:/Program Files/National Instruments/LabVIEW 8.6/Projects/CRIO Tool/FRC Images
//Then they will show up in the "Format Controller" section of the cRIO 
//Imaging Tool.

//BEN NOTE:
//All the documentation for the following classes can be found by going to
//Local Disc C, WindRiver, docs, extensions, FRC, WPI Library C++ reference.

//BEN NOTE:
//All .cpp files that have methods (not the defines.h class, which only
//has variables), must have a corresponding .h class.  The .h class
//declares methods and variables, while the .cpp class defines them.

//BEN NOTE:
//All objects that are pointers (which are most objects, as you will see in
//the .h class), have methods that should be accessed with an "->" rather
//than a ".".  This signifies that you do not want to use the method on the
//pointer itself (though there are methods that apply to pointers, I believe.)
//Rather, you want to use the method on the object that the pointer is
//pointing to.

//Do not forget this!

Robot::Robot():
_frSpeed(0),_flSpeed(0),_brSpeed(0),_blSpeed(0), _armPos(0),
_maxVolts(12.0)
{
	_init();
	
}

Robot::~Robot()
{	
	return;
}

void Robot::_init()
{

	
//----------------------------------------------------------------
// Joystick Initialization
//----------------------------------------------------------------
//this->blank is not actually necessary, beacuase there is only
//one Joy variable in one .h class.
		this->Joy = new Joystick(1); 
		this->Joy2 = new Joystick(2);

//----------------------------------------------------------------
// Jaguar Initialization
//----------------------------------------------------------------
	
// BEN NOTE:
// The "SetVoltageRampRate(double x)" method that is commented out in the
// following code can and should be used to fix the drive problems.
// It is a straightforward method, regulating how quickly the voltage
// can go up and down. Play with the numbers until you get the maximum
// rate. I believe the range if from 0-100, but don't ask me why.
		bool constsol = false;
		double ramprate = 40.0;
		JagBL = new CANJaguar(5, (CANJaguar::ControlMode)CANJaguar::kVoltage);
		Wait(0.01);
		JagBL->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagBL->ConfigFaultTime(0.7f);
		Wait(0.01);
		JagBL->EnableControl(0.0f);
		Wait(0.01);
		JagBL->ConfigMaxOutputVoltage(_maxVolts);
		JagBL->SetVoltageRampRate(ramprate);
		
		JagBR = new CANJaguar(6, (CANJaguar::ControlMode)CANJaguar::kVoltage);
		Wait(0.01);
		JagBR->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagBR->ConfigFaultTime(0.7f);
		Wait(0.01);
		JagBR->EnableControl(0.0f);
		Wait(0.01);
		JagBR->ConfigMaxOutputVoltage(_maxVolts);
		JagBR->SetVoltageRampRate(ramprate);
		
		JagFL = new CANJaguar(3, (CANJaguar::ControlMode)CANJaguar::kVoltage);
		Wait(0.01);
		JagFL->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagFL->ConfigFaultTime(0.7f);
		Wait(0.01);
		JagFL->EnableControl(0.0f);
		Wait(0.01);
		JagFL->ConfigMaxOutputVoltage(_maxVolts);
		JagFL->SetVoltageRampRate(ramprate);
		
		JagFR = new CANJaguar(4, (CANJaguar::ControlMode)CANJaguar::kVoltage);
		Wait(0.01);
		JagFR->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagFR->ConfigFaultTime(0.7f);
		Wait(0.01);
		JagFR->EnableControl(0.0f);
		Wait(0.01);
		JagFR->ConfigMaxOutputVoltage(_maxVolts);
		JagFR->SetVoltageRampRate(ramprate);
		
//BEN NOTE:
//You need to figure out how to configure the encoders. You will
//know they are properly configured once you can see them from the
//BDCComm utility, and from there PID should be simple.
//Make sure to be in Position mode in BDCComm.
		
//BEN NOTE:
//If you can't get encoders working, change kPosition to kVoltage,
//comment out the lines of code from "this->JagArm->SetPosition..."
//to the wait statement after "JagARM->SetPID....".
//If you set to kVoltage, I'm not sure how many volts you
//will have to multiply things by, so you'll have to test.
		

 		JagARM = new CANJaguar(8, (CANJaguar::ControlMode)CANJaguar::kPosRef_QuadEncoder);
		Wait(0.01);
//Will be kPosRef QuadEncoder with encoders.
		this->JagARM->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
		Wait(0.01);
		JagARM->SetPID(5.0,0.1,0.0); 
		Wait(0.01);
		JagARM->ConfigMaxOutputVoltage(_maxVolts);

		this->JagARM->ConfigEncoderCodesPerRev(200);
		Wait(0.01);
		JagARM->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagARM->ConfigFaultTime(0.5f);
		Wait(0.01);
		JagARM->EnableControl(0.0f);
		Wait(0.01);
		_armPos = JagARM->GetPosition();
		
		JagWRIST = new CANJaguar(9, (CANJaguar::ControlMode)CANJaguar::kPosition);
		Wait(0.01);
//Will be kPosRef_QuadEncoder with encoders.		
		this->JagWRIST->SetPositionReference(CANJaguar::kPosRef_None);
		Wait(0.01);
//I don't know whether or not you want to use this:
		//JagARM->ConfigEncoderCodesPerRev(<int>);
		//Wait(0.01);	
		JagWRIST->SetPID(50.0,0.0,0.0); 
		Wait(0.01);
		JagWRIST->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagWRIST->ConfigFaultTime(0.5f);
		Wait(0.01);
		JagWRIST->EnableControl(0.0f);
		Wait(0.01);
		
//Solenoids are initialized depending on their port number.
		Sol = new Solenoid(1);
		Sol2 = new Solenoid(2);

//Servos
//		cam1 = new Servo(1); 
//		cam2 = new Servo(2); 
		
//This is the compressor class.  It takes the two inputs:
//The compressor gage, and the relay that turns the compressor
//on and off.  See WPI documentation for more details.
		comp = new Compressor(1, 1); 
		
//This class directly controls the relays, in case you need to
//do so. Disregard this boolean.
		//rel = new Relay(2, Relay::kForwardOnly);
	
		
		flutter = 0.0;
		
		
		Wait(0.01);
		
		dds = new DashboardDataSender();
		Wait(0.01);
	
}

void Robot::RobotInit()
{
	this->GetWatchdog().SetEnabled(false);
	this->GetWatchdog().SetExpiration(1.0);
	
//This is an arbitrary number constant to multiply things by, as you'll see.
	VOLTS = 5;
	return;
}

double Robot::GetPeriod()
{
	return 0;
}


//----------------------------------------------------------------
// Autonomous Init
//----------------------------------------------------------------
void Robot::AutonomousInit()
{
	autonomous = 1000;//0;
	this->GetWatchdog().SetEnabled(false);
	this->GetWatchdog().Feed();
	this->GetWatchdog().SetExpiration(0.5);
	
	this->SetPeriod(0.25);
	
	return;
}


//----------------------------------------------------------------
// Autonomous Runtime loop
//----------------------------------------------------------------
void Robot::AutonomousContinuous()
{
//BEN NOTE:
//Write the autonomous mode!
	
//Feed the watch dog timer.
//Ben, I assume you understand this code, seeing as you made it.
	
	this->GetWatchdog().Feed();
	double speed = 1.5;
	double mathDir = 0;
	if(autonomous == 0) 
	{
		this->JagFL->Set(sin(mathDir)*speed * VOLTS);
		//I don't know how necessary the adjustment constant is.
		this->JagBR->Set(-sin(mathDir)*speed * VOLTS * 1.05);
		this->JagFR->Set(cos(mathDir)*speed * VOLTS);
		this->JagBL->Set(-cos(mathDir)*speed * VOLTS);
		autonomous++;
	}
	else if (autonomous >= 300 && autonomous <= 600) 
	{
		this->JagFL->Set(sin(mathDir)*speed * VOLTS);
		//I don't know how necessary the adjustment constant is.
		this->JagBR->Set(-sin(mathDir)*speed * VOLTS * 1.05);
		this->JagFR->Set(cos(mathDir)*speed * VOLTS);
		this->JagBL->Set(-cos(mathDir)*speed * VOLTS);
		autonomous++;		
//		this->JagARM->Set(.5 * VOLTS);
	}
	else if(autonomous > 600 && autonomous < 700) 
	{
		this->JagFL->Set(sin(mathDir)*speed * VOLTS);
		this->JagBR->Set(sin(mathDir)*speed * VOLTS * 1.05);
		this->JagFR->Set(cos(mathDir)*speed * VOLTS);
		this->JagBL->Set(cos(mathDir)*speed * VOLTS);
		autonomous++;
		//Sol->Set(!Sol->Get());
	}
	
	return;
}




void Robot::AutonomousPeriodic()
{
	this->GetWatchdog().Feed();
	return;
}
		

void Robot::DisabledContinuous()
{
	return;
}
		
void Robot::DisabledInit()
{
	this->GetWatchdog().SetEnabled(true);
	this->GetWatchdog().Feed();
	this->GetWatchdog().SetExpiration(1.0);

	this->SetPeriod(0.05);
	
	return;
}

void Robot::DisabledPeriodic()
{
	this->GetWatchdog().Feed();
	return;
}

void Robot::SetPeriod(double Period) 
{
	return;
}

void Robot::TeleopContinuous() 
{
	this->GetWatchdog().Feed();
	
//-------------------------------------------------------
// Camera
//-------------------------------------------------------

//One line of code that controls the camera.
	
	AxisCamera &camera = AxisCamera::GetInstance();
	if (camera.IsFreshImage()) //this whole statement is possibly unnecessary
	{
		HSLImage *image = camera.GetImage();
		vector<Target> targets = Target::FindCircularTargets(image);
		delete image;
		dds->sendVisionData(0.0, 0.0, 0.0, 0.0, targets);
	}

//-------------------------------------------------------
// Compressor
//-------------------------------------------------------
	if(Joy->GetRawButton(8)) comp->Stop();
	else if(Joy->GetRawButton(9)) comp->Start();
	if(comp->GetPressureSwitchValue() >= 120) comp->Stop();
	
//-------------------------------------------------------
//Arm and Wrist
//-------------------------------------------------------
	float MARGINOFERROR = .1;
	bool updown = (Joy2->GetDirectionDegrees() > -90 && Joy2->GetDirectionDegrees() < 90);
	float magn = Joy2->GetMagnitude();
	float speedarm = (-1 * (Joy2->GetZ()) + 1)/300  * magn;
	float speedwrist = (-1 * (Joy2->GetZ()) + 2)/5;

//BEN NOTE:
//If this becomes voltage mode, you'll replace everything with this to 
//speedarm or -speedarm * VOLTS * some constant.  I don't know what
//this constant is.  You'll probably want to clamp this value as well.
//Actually, you'll probably want to make a _spArm variable and
//_spWrist variable to store voltage values, then clamp them,
//then set them.
	
//-------------------------------------------------------
// Arm
//-------------------------------------------------------
	if(magn > MARGINOFERROR)
	{
//Go forward.
		if(updown) 
		{
			_armPos = _armPos + speedarm;// + flutter;
			//_armPos = speedarm + this->JagARM->GetPosition();//+ flutter
		}
//Go backwards.
		else
		{
			_armPos = _armPos - speedarm;// + flutter;
			if(_armPos < 0) _armPos = 0;
			//_armPos = -speedarm + this->JagARM->GetPosition();//+ flutter
		}
			//flutter *= -1;
	}
//Go nowhere.	
	else
	{	
		//_armPos = this->JagARM->GetPosition(); //+ flutter
		//_armPos = _armPos;// + flutter;
	}
//-------------------------------------------------------
// Wrist
//-------------------------------------------------------
//Button 2 is down; Button 3 is up
	if(Joy2->GetRawButton(3))
		this->JagWRIST->Set(-speedwrist + this->JagWRIST->GetPosition());
	else if(Joy2->GetRawButton(2))
		this->JagWRIST->Set(speedwrist + this->JagWRIST->GetPosition());
	else
		this->JagWRIST->Set(0);
	
//-------------------------------------------------------
// Solenoid
//-------------------------------------------------------
	if(Joy2->GetRawButton(1))
	{
		if(constsol)
		{
			Sol->Set(!Sol->Get());
		}
		if(constsol == true) constsol = false;
	}
	else
	{
		constsol = true;
	}

	if(Joy->GetRawButton(10))
	{
		Sol2->Set(true);
	}
	if(Joy->GetRawButton(11)) 
	{
		Sol2->Set(false);
	}

//-------------------------------------------------------
// Camera Servos
//-------------------------------------------------------
/*	if(Joy2->GetRawButton(6))cam1->Set(cam1->Get() + 0.05);
	else if(Joy2->GetRawButton(7))cam1->Set(cam1->Get()-0.05);
	else cam1->Set(cam1->Get());
	if(Joy2->GetRawButton(11))cam2->Set(cam2->Get() + 0.05);
	else if(Joy2->GetRawButton(10))cam2->Set(cam2->Get()-0.05);
	else cam2->Set(cam2->Get());
*/

//-------------------------------------------------------
// Driving Code
//-------------------------------------------------------
	float dir = Joy->GetDirectionDegrees();
	float mag = Joy->GetMagnitude();
	float speed = (-1 * (Joy->GetZ()) + 2) * mag;
	float rotspeed = (-1 * (Joy->GetZ()) + 2) * 0.4;

//BEN NOTE:
//Pi is defined as acos(-1).
	
	float mathDir = -Joy->GetDirectionRadians() - acos(-1)/4 + acos(-1);
	 
//Omnidirectional Drive
	if (mag > 0.1)
	{
		_flSpeed = sin(mathDir)*speed * VOLTS;
		_brSpeed = -sin(mathDir)*speed * VOLTS * 1.05;
		_frSpeed = cos(mathDir)*speed * VOLTS;
		_blSpeed = -cos(mathDir)*speed * VOLTS;
	}
	
	else
	{
		_flSpeed = 0.0f;
		_brSpeed = 0.0f;
		_frSpeed =0.0f;
		_blSpeed = 0.0f;
	}
	
//Rotate Clockwise
	if (Joy->GetRawButton(5))
	{
		_flSpeed = rotspeed * VOLTS;
		_brSpeed = rotspeed * VOLTS;
		_frSpeed =rotspeed * VOLTS;
		_blSpeed = rotspeed * VOLTS;
		
	}
	
//Rotate Counterclockwise
	else if (Joy->GetRawButton(4))
	{
		_flSpeed = -rotspeed * VOLTS;
		_brSpeed = -rotspeed * VOLTS;
		_frSpeed =-rotspeed * VOLTS;
		_blSpeed = -rotspeed * VOLTS;
	}	

//Clamp the speeds down to a number between -12.0 and 12.0 volts. See class.
	clampSpeeds();
	
	Wait(0.01);
	
	return;
}


//This class references the clamp(float x) down below.

void Robot::clampSpeeds()
{
	this->GetWatchdog().Feed();
//	if (_armPos < 0) _armPos = 0; 
	//if (_armPos > 1) _armPos = 1;
	clamp(_flSpeed);
	clamp(_brSpeed);
	clamp(_frSpeed);
	clamp(_blSpeed);
		
	return;
}

void Robot::TeleopInit()
{
	this->GetWatchdog().SetExpiration(1.0);
	this->GetWatchdog().SetEnabled(true);
	this->GetWatchdog().Feed();
	
	AxisCamera &camera = AxisCamera::GetInstance();
	camera.WriteResolution(AxisCamera::kResolution_320x240);
	camera.WriteCompression(20);
	camera.WriteBrightness(0);

	this->SetPeriod(0.1);
	if(comp) comp->Start();
	
	return;
}
//
//The speeds are actually set PERIODICALLY.  This is important, because
//it makes sure that the robot doesn't get overloaded. (Or so says Nick.)

void Robot::TeleopPeriodic() 
{
	this->GetWatchdog().Feed();
	
//	this->JagARM->Set(_armPos);
	this->JagFL->Set(_flSpeed);
	this->JagBR->Set(_brSpeed);
	this->JagFR->Set(_frSpeed);
	this->JagBL->Set(_blSpeed);
	return;
}

//C++ is very special.  You are able to give it a reference to a variable
//as an argument, and then use that variable itself in the actual method.
//(By the way, methods are called functions in C++). See example below:
void Robot::clamp(float & val)
{
	if(val < -12.0f) val = -12.0f;
	if(val > 12.0f)	val = 12.0f;
}

START_ROBOT_CLASS(Robot);
